package ece496.sensorService;

import java.io.File;
import java.io.FileWriter;
import java.io.IOException;

import android.os.Environment;
import android.util.Log;
import ioio.lib.api.AnalogInput;
import ioio.lib.api.DigitalOutput;
import ioio.lib.api.IOIO;
import ioio.lib.api.exception.ConnectionLostException;

public class IOIOThread extends Thread {
    //constructor
    boolean done_flag = false;
    final double[] sample_center = new double[5];
    final double[] sample_left = new double[5];
    final double[] sample_right = new double[5];
    double last_l_reading = 0;
    double last_c_reading = 0;
    double last_r_reading = 0;
    String output_str = "output_str";
    private IOIO ioio;
    int center_reading_id = 0;
    int left_reading_id = 0;
    int right_reading_id = 0;
    //private PwmOutput pwmOutput_;
    //private String enterStr;
    //final audioClass audio;
    //pkg496_spu.spu spu = new pkg496_spu.spu();
    double center_output;
    double left_output;
    double right_output;
    final int[] center_play_flags = {0, 0, 0, 0, 0};
    final int[] left_play_flags = {0, 0, 0, 0, 0};
    final int[] right_play_flags = {0, 0, 0, 0, 0};
    int sleeptime = 80;
    double battery_volt = 0;
    double battery_perc = 100;

    public void init(IOIO ioinstance) {
        ioio = ioinstance;
    }

    public String output() {
        return output_str;
    }

    public synchronized void finish() {
        done_flag = true;
    }

    public double getlastreading(int sensor)
    {
    	switch (sensor)
    	{
    	case 0: return last_l_reading;
    	case 1: return last_c_reading;
    	case 2: return last_r_reading;
    	default: return -1;
    	}
    }
    public synchronized boolean done() {
        return done_flag;
    }
    
    public synchronized double battery() {
    	ece496.pkg496_spu.battery battery = new ece496.pkg496_spu.battery();
    	battery_perc = battery.battery(battery_volt);
    	
    	return battery_perc;
    }

    @Override
    public final void run() {
        Log.d(getClass().getSimpleName(), "Thread Running");
        writeToCard("started\n", "thread.txt");
        AnalogInput sensor_center = null;
        AnalogInput sensor_left = null;
        AnalogInput sensor_right = null;
        DigitalOutput rx_center = null;
        DigitalOutput rx_side = null;
        AnalogInput knob = null;
        AnalogInput monitor = null;
        done_flag = false;
        
        long startTime;
        long endTime;
        long exeTime;
 
        try {

            //  input_ = ioio_.openAnalogInput(31);
            //	pwmOutput_ = ioio_.openPwmOutput(12, 100);
            sensor_center = ioio.openAnalogInput(33);
            sensor_left = ioio.openAnalogInput(32);
            sensor_right = ioio.openAnalogInput(34);

            rx_center = ioio.openDigitalOutput(11, DigitalOutput.Spec.Mode.OPEN_DRAIN, false);
            rx_side = ioio.openDigitalOutput(10, DigitalOutput.Spec.Mode.OPEN_DRAIN, false);
            
            rx_center.write(true);
            rx_side.write(false);
            
            knob = ioio.openAnalogInput(37);
            monitor = ioio.openAnalogInput(38);

            sleep(sleeptime);
            
        } catch (ConnectionLostException e) {
        } catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
        
        writeToCard("Initialized, starting loop\n", "thread.txt");
        writeToCard("done = " + String.valueOf(done()), "thread.txt");
        //while loop here
        
        double knob_val = 0;
        double threshold = 0.3;
        
        while (done_flag == false) {
            writeToCard("inside loop\n", "thread.txt");
            //enableUi(false);
            try {
            	
            	knob_val = knob.read();
            	threshold = 0.35;
            	
            	battery_volt = monitor.getVoltage();
            	
                startTime = System.nanoTime();
                
                try {
                	center_reading_id = sensorReading(sample_center, sensor_center, center_play_flags, center_reading_id, 1, threshold);

                	rx_center.write(false);
                	rx_side.write(true);
                } finally {
                    endTime = System.nanoTime();
                }
                
                exeTime = (long) ((endTime - startTime) / 1.0E6);

                //writeToCard("Center: " + Long.toString(exeTime), "exe_timing.txt");
                
                /*if  (exeTime > 0 && exeTime <= sleeptime) {
                	sleep(sleeptime - exeTime);
                } else if (exeTime <= 0) {
                	*/
                sleep(sleeptime);
                //}

                startTime = System.nanoTime();                

                try {
                	left_reading_id = sensorReading(sample_left, sensor_left, left_play_flags, left_reading_id, 0, threshold); //playsound(2)                sample_right[right_reading_id] = sensor_right.read();
                	right_reading_id = sensorReading(sample_right, sensor_right, right_play_flags, right_reading_id, 2, threshold);//playsound(2)
                	
                    rx_center.write(true);
                    rx_side.write(false);
                } finally {
                    endTime = System.nanoTime();
                }
                
                exeTime = (long) ((endTime - startTime) / 1.0E6);
                
                //writeToCard("Side: " + Long.toString(exeTime), "exe_timing.txt");
                
                //if  (exeTime > 0 && exeTime <= sleeptime) {
                //	sleep(sleeptime - exeTime);
                //} else if (exeTime <= 0) {
                	sleep(sleeptime);
                //}

            } catch (InterruptedException e) {
                ioio.disconnect();
            } catch (ConnectionLostException e) {
                //enableUi(false);
            }
        }
        Log.e(getClass().getSimpleName(), "Thread Finished");
    }

    private void writeToCard(String data, String filename) {
        try {
            File root = Environment.getExternalStorageDirectory();
            if (root.canWrite()) {
                File dataFile = new File(root, filename);
                FileWriter dataWriter = new FileWriter(dataFile, true);
                dataWriter.append(data + "\n");
                dataWriter.close();
            } else {
                Log.i("SENSORIO", "cannot write to sdcard\n");
            }
        } catch (IOException e) {
            Log.e("tag", "Could not write to file " + e.getMessage());
        }
    }
    ///////////////////////
    //sensor: left = 0
    //		   centre = 1
    //		   right = 2
    /////////////////////

    private int sensorReading(double[] sample_center, AnalogInput pin, int[] play_flags, int readingID, int sensor, double threshold) throws InterruptedException, ConnectionLostException {
        ece496.pkg496_spu.spu spu = new ece496.pkg496_spu.spu();
        sample_center[readingID] = pin.read();
        double output = 0;
        writeToCard("length = " + sample_center.length, "thread.txt");
        String tmp_out = "";
        //Not very useful here, but we need it for testing
        switch (sensor) {
            case 0:
                tmp_out = "left:";
                break;
            case 1:
                tmp_out = "centre:";
                break;
            case 2:
                tmp_out = "right:";
                break;
        }

        if (readingID == sample_center.length - 1) {
        	if (sample_center[readingID] > threshold)
        		sample_center[readingID] = threshold + 0.1;
            output = spu.spu(sample_center, threshold);
            writeToCard("output = " + output + "\n", "thread.txt");
            readingID = 0;

            if (output == -1) {
                tmp_out += " > 3";
            } else {
                tmp_out += Double.toString(output) + "m";
            }
            switch (sensor) {
            case 0:
            	if (Math.abs(last_l_reading - output) > 0.1)
                last_l_reading = output;
                break;
            case 1:
            	if (Math.abs(last_c_reading - output) > 0.1)
                last_c_reading = output;
                break;
            case 2:
            	if (Math.abs(last_r_reading - output) > 0.1)
                last_r_reading = output;
                break;
        }
        }
        //this is where we would be storing the values
     
        readingID++;
        String writestr = "last_L = " + last_l_reading + " last_C:" + last_c_reading
        				+ " last R: " + last_r_reading + "\n";
        writeToCard(writestr, "thread.txt");
        return readingID;
    }
}